#include <iostream>
#include <ros/ros.h>
//msgs type and conversion
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
//point types
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <Eigen/Core>

using namespace std;
using namespace cv;

ros::Publisher pcl_pub;
ros::Publisher obj_pub;

Mat inputImg;
void img_cb(const sensor_msgs::ImageConstPtr& msg)
{
    cv_bridge::CvImagePtr cv_ptr;  //申明一个CvImagePtr
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    inputImg = cv_ptr->image;
}


void pcl_call_back(const sensor_msgs::PointCloud2ConstPtr& input)
{
//    Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
    pcl::PointCloud<pcl::PointXYZ> inputCloud;
    pcl::fromROSMsg (*input, inputCloud);//cloud is the output

//    pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
//    // Perform the actual filtering
//    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
//    // build the filter
//    sor.setInputCloud(inputCloud.makeShared());
//    sor.setMeanK(10);
//    sor.setStddevMulThresh(1);
//    // apply filter
//    sor.filter(cloud_filtered);

    pcl::PointCloud<pcl::PointXYZ> outputCloud;
    float x = 0;
    float y = 0;
    float z = 0;
    float dis = 0;
    float dis2d = 0;
    double sum_x = 0;
    double sum_y = 0;
    double sum_z = 0;
    for(int i =0;i<inputCloud.points.size();++i)
    {
        x = inputCloud.points[i].x;//-1;//+1;
        y = inputCloud.points[i].y;//-0.1;//+0.2;//
        z = inputCloud.points[i].z;

        dis = sqrt(x*x+y*y+z*z);
        dis2d = sqrt(x*x+y*y);
//        if(dis>0.6||dis<0.5||z>-0.3||dis2d>0.2)
        if(x )
        {

            continue;
        } else
        {
            outputCloud.points.push_back(inputCloud.points[i]);
            sum_x += inputCloud.points[i].x;
            sum_y += inputCloud.points[i].y;
            sum_z += inputCloud.points[i].z;
        }
    }

    if(!outputCloud.points.empty())
    {
        sum_x /= outputCloud.points.size();
        sum_y /= outputCloud.points.size();
        sum_z /= outputCloud.points.size();


        float cx_ =261.5;
        float cy_ =149.5;
        float fx_ = 150.0;
        float fy_ = 150.0;
        float u = sum_x*fx_+cx_;
        float v = sum_y*fy_+cy_;
        if(!inputImg.empty())
        {
//            cout<<Point(u,v)<<endl;
            circle(inputImg,Point(u,v),6,Scalar(255),-1);
            imshow("Img", inputImg);
            waitKey(1);
        }


        Eigen::Vector3d obj_point(sum_x,sum_y,sum_z);
//        cout<<"obj_point"<<endl<<obj_point<<endl;
//    obj_pub.publish(obj_point);
        sensor_msgs::PointCloud2 output;
        pcl::toROSMsg(outputCloud,output);
        output.header.frame_id = "map";
        pcl_pub.publish(output);
    }

}

int main(int argc,char** argv)
{
// Initialize ROS
    ros::init (argc, argv, "pcl_2_pcd");
    ros::NodeHandle nh;

// Create a ROS subscriber for the input point cloud
    ros::Subscriber sub = nh.subscribe ("/bi2pc/camera/PointCloud", 1, pcl_call_back);
    ros::Subscriber sub_img = nh.subscribe("/cam/left",1,img_cb);
    pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("filtered_pcl",1);
//    obj_pub = nh.advertise<Eigen::Vector3d>("obj_pos",1);
// Spin

    while(!ros::isShuttingDown()) {
//        if(!inputImg.empty()) {
//
//        }
        ros::spinOnce();
    }
}
